class Hinge implements Constraint{
  
  private Sphere s1,s2,s3,s4,s5,s6;
  private float angulo;
  private float distanciaC;
  private PVector h1,h2,h3,h4;
  private PVector ni,nd;
  private PVector p2;
  
  
 public Hinge(Sphere s1, Sphere s2, Sphere s3, float angulo,Sphere s4, Sphere s5, Sphere s6){
  this.s1=s1;
  this.s2=s2;
  this.s3=s3;
  this.s4=s4;
  this.s5=s5;
  this.s6=s6;
  this.angulo = angulo;
  this.ni = new PVector(-1,1,0);
  this.nd = new PVector(1,-1,0);
  
  float distancia12 =s1.getVector().dist(s2.getVector());
  float distancia23 = s3.getVector().dist(s2.getVector());
 // println("distancia12: "+distancia23);
  distanciaC = sqrt(distancia12*distancia12+distancia23*distancia23-(2*distancia12*distancia23*cos(radians(angulo))));
  //println("distasncia c: "+distanciaC);
 // println(height);
 }
 

 
 public boolean apply(){
 
 PVector A = s1.getVector();
 PVector C = s2.getVector();
 PVector B = s3.getVector();
 
 float a = C.dist(B);
 float b = C.dist(A);
 float c; // no se sabe
 float cAct; 
 // Si el angulo es angulo...
 c = sqrt(a*a+b*b-(2*a*b*cos(radians(angulo))));
 float c2 = sqrt(a*a+b*b-(2*a*b*cos(radians(30))));
 cAct = A.dist(B);
 
 if(cAct>c ){
 float diff = abs(cAct-c);
 diff=diff/c;
   //println("aplico: "+diff);
   //B.add(new PVector(0,diff/2,0));
   //A.sub(new PVector(0,diff/2,0));
   
  float dista = sqrt(pow(A.x -B.x,2)+pow(A.y-B.y,2)+pow(A.z-B.z,2));
  float dx = A.x-B.x;
  float dy = A.y-B.y;
  float dz = A.z-B.z;
  
  diff = cAct-c;
  diff = diff/c;
  
  
  
  s1.setPosicion(new PVector(A.x -(diff*dx),A.y -(diff*dy),A.z -(diff*dz)));
  
  s3.setPosicion(new PVector(B.x +(diff*dx),B.y +(diff*dy),B.z +(diff*dz)));
 //s1.setPosicion(s1.getPosicionAnterior());
 //s3.setPosicion(s3.getPosicionAnterior());
   
   return true;
 }
 else if(cAct < c2){
   
   float diff = abs(cAct-c);
 diff=diff/c;
   //println("aplico: "+diff);
   //B.add(new PVector(0,diff/2,0));
   //A.sub(new PVector(0,diff/2,0));
   
  float dista = sqrt(pow(A.x -B.x,2)+pow(A.y-B.y,2)+pow(A.z-B.z,2));
  float dx = A.x-B.x;
  float dy = A.y-B.y;
  float dz = A.z-B.z;
  
  diff = cAct-c2;
  diff = diff/c2;
  
  
  
  s1.setPosicion(new PVector(A.x -(diff*dx),A.y -(diff*dy),A.z -(diff*dz)));
  
  s3.setPosicion(new PVector(B.x +(diff*dx),B.y +(diff*dy),B.z +(diff*dz)));
   
   return true;
 }
 
 return false;
 
}
 
 public void draw(){
  
}
}
